﻿//////////////////////////////////////////////////////////////////////
// <copyright file=Reconstructor.cs">
//     Copyright (c) 2010
//     Released under  GNU GENERAL PUBLIC LICENSE
//     http://dev.perl.org/licenses/gpl1.html
// </copyright>
// <date> 3/30/2010 1:10:51 PM </date>
// <author> B. Endrovski </author>
// <web> http://furiouspixels.blogspot.com/ </web>
//////////////////////////////////////////////////////////////////////

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Drawing;

namespace ContourReconstruction
{	
	public class Reconstructor
	{
		// List of triangles
		private List<Triangle> triangles;

		// Array of point given point (input)
		private Vector2D[] points;

		// Next point to add
		private int next = 0;

		// Super triangle
		private Vector2D sA;		// super A
		private Vector2D sB;		// super B
		private Vector2D sC;		// super C

		// A heap for the trinagles mark for (removal) sculpting
		private BinaryMaxHeap<Triangle> heap;
		
		/// <summary>
		/// All the edges of the triangle mesh
		/// </summary>
		public IEnumerable<Edge> Edges
		{
			get { return edges.Values;	}
		}
		// Stored in a dicitionary for fast access and no duplicates
		private Dictionary<int, Edge> edges;

		/// <summary>
		/// Generated collection of the edges if the contour
		/// </summary>
		public IEnumerable<Edge> ContourEdges
		{
			get
			{
				foreach (Edge e in edges.Values)
					if (e.IsExterior)
						yield return e;
			}
		}

		/// <summary>
		/// Contour vertices
		/// </summary>
		public IEnumerable<Vector2D> ContourVertices
		{
			get
			{
				return contourVertices.AsEnumerable();
			}
		}
		// Stored in a set for fast access and no duplicates
		private HashSet<Vector2D> contourVertices;				

		public bool IsDelaunay
		{ get; private set; }

		public bool IsDone
		{ get; private set; }

		public List<Edge> ActiveEdges { get; private set; }
		public List<Triangle> ActiveTriangles { get; private set; }

		public Reconstructor(Vector2D[] points)
		{
			this.points = points;
			//
			edges = new Dictionary<int, Edge>();

			IsDelaunay = false;
			IsDone = false;

			triangles = new List<Triangle>();
			ActiveTriangles = new List<Triangle>();
			ActiveEdges = new List<Edge>();

			// Init super triangle
			float xmin = Single.MaxValue;
			float ymin = Single.MaxValue;
			//
			float xmax = Single.MinValue;
			float ymax = Single.MinValue;
			//
			for (int i = 0; i < points.Length; i++)
			{
				Vector2D p = points[i];
				//
				if (p.X < xmin) xmin = p.X;
				if (p.X > xmax) xmax = p.X;
				if (p.Y > ymax) ymax = p.Y;
				if (p.Y < ymin) ymin = p.Y;
			}
			//
			float dx = xmax - xmin;
			float dy = ymax - ymin;
			float dmax = (dx > dy) ? dx : dy;
			//
			float xmid = (xmax + xmin) * 0.5f;
			float ymid = (ymax + ymin) * 0.5f;
			//
			sA = new Vector2D(xmid - 2 * dmax, ymid - dmax);	// super A
			sB = new Vector2D(xmid, ymid + 2 * dmax);			// super B
			sC = new Vector2D(xmid + 2 * dmax, ymid - dmax);	// super C
			//
			triangles.Add(new Triangle(sA, sB, sC));
			RecalcEdges();
		} // end constructor

		private bool IsSuperTriangle(Triangle t)
		{
			foreach (Vector2D v in t.Vertices)
				if (v == sA || v == sB || v == sC)
					return true;
			return false;
		}
		
		private void RecalcEdges()
		{
			edges.Clear();
			// Refresh edges from triangles
			// add parent triangle to edge, double ones are inside edges
			foreach (Triangle t in triangles)
			{
				if (IsSuperTriangle(t))												// Ignore super triangles
					continue;

				for (int i = 0; i < 3; i++)
				{
					Edge e = new Edge(t.Vertices[i], t.Vertices[(i + 1) % 3]);		// Create edge
					int key = e.GetHashCode();										// Get key

					if (!edges.ContainsKey(key))									// Retrieve original edge if any
						edges.Add(key, e);											// Add if not
					edges[key].Triangles.Add(t);									// Add this triangle to it
				}				
			}
		}

		public void NextStep()
		{
			if (!IsDelaunay)
				DelaunayStep();
			else if (heap == null)
				CreatHeap();
			else if (!IsDone)
				SculpStep();
		}

		/// <summary>
		/// Each step adds a point to the trinagulation, the triangulation satisfies
		/// the Delaunay rule prior to the addition of the point.
		/// </summary>
		public void DelaunayStep()
		{
			// No more points to add
			if (next >= points.Length)
			{
				IsDelaunay = true;
				return;
			}

			ActiveTriangles.Clear();
			ActiveEdges.Clear();
			edges.Clear();

			// Edges that will be used in the new triangulation of the local polygon
			HashSet<Edge> edgeBuffer = new HashSet<Edge>();
			Vector2D v = points[next++];

			// Goes through the triangles. Retrieves all the triangles that violate the Delaunay.
			// Creates a polygon from their union and retriangulates the resulting polygon.
			for (int j = 0; j < triangles.Count; j++)
			{
				Triangle t = triangles[j];				
				//
				if (v.Distance(t.Circumcenter) < t.Circumradius)
				{
					for (int i = 0; i < 3; i++)
					{
						// Create edge
						Edge e = new Edge(t.Vertices[i], t.Vertices[(i + 1) % 3]);
						//
						if (edgeBuffer.Contains(e))					// double edge (interior)
							edgeBuffer.Remove(e);					// so remove
						else
							edgeBuffer.Add(e);
					}
					
					ActiveTriangles.Add(t);
					triangles.Remove(t);							// Remove t from triangulation
					j--;
				}				
			}

			// Trainguluate the the local polygon
			foreach (Edge e in edgeBuffer)
			{
				triangles.Add(new Triangle(e.A, e.B, v));
			}

			RecalcEdges();			
			ActiveEdges = new List<Edge>(edgeBuffer);
		} // end method		

		private void CreatHeap()
		{
			heap = new BinaryMaxHeap<Triangle>(triangles.Count);			

			ActiveTriangles.Clear();
			ActiveEdges.Clear();
			//
			triangles.RemoveAll(IsSuperTriangle);								// Quick remove of all!
			//
			foreach (Triangle t in triangles)
			{
				for (int i = 0; i < 3; i++)
				{
					// Create edge
					Edge e = new Edge(t.Vertices[i], t.Vertices[(i + 1) % 3]);
					int key = e.GetHashCode();									
					t.Neighbors[(i + 2) % 3] = edges[key].GetNeighbour(t);		// Oposite of the edge					
				}

				if (t.NeighborCount == 2)
				{
					t.M = Metric(t);
					heap.Enqueue(t);
				}
			}

			RecalcEdges();
			contourVertices = new HashSet<Vector2D>();
			foreach (Edge e in ContourEdges)
			{
				contourVertices.Add(e.A);
				contourVertices.Add(e.B);
			}
		}

		/// <summary>
		/// Checks if all three vertices are on the boundary the vertex
		/// </summary>
		private bool IsKeyTriangle(Triangle t)
		{			
			int c = 0;
			foreach (Vector2D v in t.Vertices)
				if (contourVertices.Contains(v)) c++;
			return c > 2;
		}

		private float Metric(Triangle t)
		{		
			// Find the exterior edge
			int i;
			for (i = 0; i < 3; i++)
				if (t.Neighbors[i] == null)
					break;

			// This triangle has no exterior edge
			if (i == 3)
				return 0.0f;	// Metric is always positive

			// Get the vertices
			Vector2D A = t.Vertices[(i + 1) % 3];
			Vector2D B = t.Vertices[(i + 2) % 3];
			Vector2D C = t.Vertices[i];

			// All calculations in local coordinate with origin
			// in the Circumcenter
			Vector2D mid = (A + B) * 0.5f;					// mid point in world coordinates
			Vector2D toCenter = (t.Circumcenter - mid);

			// If mid point is on center, the distance is simply R 
			if (toCenter.Length <= 0.0001f)
				return t.Circumradius;

			toCenter.Normalize();							// We the dirction and unit length
			Vector2D inters = toCenter * t.Circumradius;	// Intresection in local coordiantes 

			Vector2D normal = (A - B).Perp;					// Normal of the edge
			Vector2D toC = (C - mid);

			// Check if the intersection and third point are on the
			// same side of the edge, invert sign if not
			if (normal.Dot(toCenter) * normal.Dot(toC) > 0.0f)
				inters *= -1.0f;
 
			inters += t.Circumcenter;						// Intresection in world coordiantes

			Vector2D dist = (inters - mid);			

			// Method visualization
			Edge e = new Edge(mid, inters);
			ActiveEdges.Add(e);
			e = new Edge(A, B);
			ActiveEdges.Add(e);
			ActiveTriangles.Add(t);

			// All done
			return dist.Length;
		}

		private void SculpStep()
		{
			if (heap.IsEmpty || contourVertices.Count == points.Length)
			{
				IsDone = true;
				return;
			}

			ActiveTriangles.Clear();
			ActiveTriangles.AddRange(heap);
			ActiveEdges.Clear();			

			Triangle t = heap.DequeueMax();
			
			// Don't delete key triangles
			if(IsKeyTriangle(t))
				return;

			// Delete the rest			
			triangles.Remove(t);

			contourVertices.Add(t.Vertices[0]);
			contourVertices.Add(t.Vertices[1]);
			contourVertices.Add(t.Vertices[2]);
			
			for (int i = 0; i < 3; i++)
			{
				Triangle nb = t.Neighbors[i];
				Edge e = new Edge(t.Vertices[(i + 1) % 3], t.Vertices[(i + 2) % 3]);
				e = edges[e.GetHashCode()];
				e.Triangles.Remove(t);

				if (nb != null)
				{					
					// is not to be added to the queue		
					if (IsKeyTriangle(nb))
						continue;

					// Remove refernces to the removed triangle
					for (int j = 0; j < 3; j++)
						if (nb.Neighbors[j] == t) nb.Neighbors[j] = null;

					// Add neighbor to queue
					nb.M = Metric(nb);
					heap.Enqueue(nb);
				}
				else
				{
					// Remove exterior edge					
					edges.Remove(e.GetHashCode());
				}
			} // end for
		} // end method
	} // end class
}
